﻿// Inner Fire 游戏引擎库
// Constraint - 物理约束
//
// Copyright (c) 2025 by 尔西大帝. All rights reserved.
//
// 所属层级：应用层（4）
// 用于解决碰撞中的穿透问题。
//
// Author: 尔西大帝
// Email: 2670613871@qq.com
// Created: 2025-06-28

#include "constraint.h"
#include <game/physics/rigid_body.h>

namespace ifire::game {
Constraint::Constraint() : bias(0.0f) {}

MatMN<12, 12> Constraint::GetInvM() const {
  // 12x12矩阵 (2个物体 × 6自由度)
  MatMN<12, 12> invM;
  invM.Zero();

  // 物体A的逆质量部分 (左上6x6块)
  // 线性部分
  invM[0][0] = a->inv_mass_; // x方向
  invM[1][1] = a->inv_mass_; // y方向
  invM[2][2] = a->inv_mass_; // z方向

  // 角部分 (使用世界空间逆惯性张量)
  const XMFLOAT3X3& invIA = a->GetInvInertiaWorld();
  invM[3][3] = invIA._11; // (0,0)
  invM[3][4] = invIA._12; // (0,1)
  invM[3][5] = invIA._13; // (0,2)

  invM[4][3] = invIA._21; // (1,0)
  invM[4][4] = invIA._22; // (1,1)
  invM[4][5] = invIA._23; // (1,2)

  invM[5][3] = invIA._31; // (2,0)
  invM[5][4] = invIA._32; // (2,1)
  invM[5][5] = invIA._33; // (2,2)

  // 物体B的逆质量部分 (右下6x6块)
  // 线性部分
  invM[6][6] = b->inv_mass_;
  invM[7][7] = b->inv_mass_;
  invM[8][8] = b->inv_mass_;

  // 角部分
  const XMFLOAT3X3& invIB = b->GetInvInertiaWorld();
  invM[9][9] = invIB._11;  // (0,0)
  invM[9][10] = invIB._12; // (0,1)
  invM[9][11] = invIB._13; // (0,2)

  invM[10][9] = invIB._21;  // (1,0)
  invM[10][10] = invIB._22; // (1,1)
  invM[10][11] = invIB._23; // (1,2)

  invM[11][9] = invIB._31;  // (2,0)
  invM[11][10] = invIB._32; // (2,1)
  invM[11][11] = invIB._33; // (2,2)

  return invM;
}

VecN<12> Constraint::GetVelocities() const {
  VecN<12> v;

  // 物体A的线性速度
  v[0] = a->velocity_.x; // x
  v[1] = a->velocity_.y; // y
  v[2] = a->velocity_.z; // z

  // 物体A的角速度
  v[3] = a->angular_velocity_.x; // ωx
  v[4] = a->angular_velocity_.y; // ωy
  v[5] = a->angular_velocity_.z; // ωz

  // 物体B的线性速度
  v[6] = b->velocity_.x; // x
  v[7] = b->velocity_.y; // y
  v[8] = b->velocity_.z; // z

  // 物体B的角速度
  v[9] = b->angular_velocity_.x;  // ωx
  v[10] = b->angular_velocity_.y; // ωy
  v[11] = b->angular_velocity_.z; // ωz

  return v;
}



} // namespace ifire::game